from mpython import *
from machine import Pin
import machine
import time
class TPBOT(object):

 
    def __init__(self,i2c):
        self._i2c = i2c
        self._addr = 0x10
        
        


        
    def set_wheel(self,lspeed,rspeed):
        if ((lspeed < 0) and (rspeed < 0)):
            lspeed = lspeed * -1
            rspeed = rspeed * -1
            self._i2c.writeto(self._addr, (bytearray([0x01,lspeed,rspeed,0x03])))            
        else:
            if (lspeed < 0):
                lspeed = lspeed * -1
                rspeed = rspeed
                self._i2c.writeto(self._addr, (bytearray([0x01,lspeed,rspeed,0x01])))
            elif (rspeed < 0):
                lspeed = lspeed
                rspeed = rspeed * -1
                weightedValue = 0x02
                self._i2c.writeto(self._addr, (bytearray([0x01,lspeed,rspeed,0x02])))
            else:            
                self._i2c.writeto(self._addr, (bytearray([0x01,lspeed,rspeed,0x00])))
     

    def set_travel_speed(self,direc,speed):
        if (direc == 0):
            lspeed = speed
            rspeed = speed       
        if (direc == 1):
            lspeed = -speed
            rspeed = -speed
        if (direc == 2):
            lspeed = -speed
            rspeed = speed
        if (direc == 3):
            lspeed = speed
            rspeed = -speed
        self.set_wheel(lspeed,rspeed)


    def set_travel_time(self,direc,speed,time):
        if (direc == 0):
            lspeed = speed
            rspeed = speed       
        if (direc == 1):
            lspeed = -speed
            rspeed = -speed
        if (direc == 2):
            lspeed = -speed
            rspeed = speed
        if (direc == 3):
            lspeed = speed
            rspeed = -speed
        self.set_wheel(lspeed,rspeed)
        time.sleep(time)
        self.stop_car()


    def stop_car(self):
        self._i2c.writeto(self._addr, (bytearray([0x01, 0, 0, 0])))



    def headlight_color(self,color):
        r = color >> 16
        g = (color >> 8) & 0xFF
        b = color & 0xFF
        self._i2c.writeto(self._addr, (bytearray([0x20, r, g, b])))



    def headlight_rgb(self,r,g,b):
        self._i2c.writeto(self._addr, (bytearray([0x20, r, g, b])))



    def headlight_close(self):
        self._i2c.writeto(self._addr, (bytearray([0x20, 0, 0, 0])))
    
    
    def get_distance(self,int_unit):
        echoTimeout = 23200
        trigPin = Pin(Pin.P16, Pin.OUT)
        echoPin = Pin(Pin.P15, Pin.IN)
        trigPin.value(0) 
        trigPin.value(1)
        time.sleep_us(10)
        trigPin.value(0)
        pulseTime = machine.time_pulse_us(echoPin, 1, echoTimeout)
        try:
            if pulseTime > 0:
                if int_unit == 0:
                    distance = round((pulseTime / 58),1)
                    str_unit = 'cm'
                else:
                    distance = round((pulseTime / 147.32),2)
                    str_unit = 'in'
                   
            if distance > 0:
                print('Distance: ', distance, str_unit)
                return distance
        except:
            print('Out of Ultrasonic Measurement Range.')
            return -1
            

    def get_tracking(self, side):
        pin_l = Pin(Pin.P13, Pin.IN, None)
        pin_r = Pin(Pin.P14, Pin.IN, None)      
        tracking_l = pin_l.value()
        tracking_r = pin_r.value()
        if side == 0:
            return tracking_l
        elif side == 1:
            return tracking_r
        else:
            return -1


    def is_track_side(self, side, state):
        if self.get_tracking(side) == state:
            return True
        else:
            return False            



    def set_servo180(self, servo, angle):
        if servo == 0:
            servo_addr = 0x10
        elif servo == 1:
            servo_addr = 0x11
        elif servo == 2:
            servo_addr = 0x12
        elif servo == 3:
            servo_addr = 0x13
        self._i2c.writeto(self._addr, (bytearray([servo_addr, angle, 0, 0])))
    
    
    
    def set_servo360(self, servo, speed):
        speed = round(numberMap(speed, -100, 100, 0, 180))
        if servo == 0:
            servo_addr = 0x10
        elif servo == 1:
            servo_addr = 0x11
        elif servo == 2:
            servo_addr = 0x12
        elif servo == 3:
            servo_addr = 0x13
        print(speed)
        self._i2c.writeto(self._addr, (bytearray([servo_addr, speed, 0, 0])))